#include "mode_chose.h"
#include <thread>


/*/
 * File: mode_chose.cpp
 * Description: Update the function of the motor
 * Version: 2.0.0
 * Date: 2025-03-25
 * Author: LowerBatter
 * Company: JuMent
/*/

int SingleMotorMovingControl(int MotorId, int TargetPosition, int velocity){
    int acc = 500000;       // 加速度
    int dec = 500000;       // 减速度
    bool isRelative = false;
    bool islmmediately = true;

    int result = harmonic_profilePositionControl(canid, MotorId, TargetPosition * turn, velocity, acc, dec, isRelative, islmmediately);

    return result;
}

// 电机归位函数
int MotorsMoveBack(){
    int acc = 500000;       // 加速度
    int dec = 500000;       // 减速度
    bool isRelative = false;
    bool islmmediately = true;

    bool result1 = harmonic_profilePositionControl(Motor1, 0, vel_4, acc, dec, isRelative, islmmediately);
    bool result2 = harmonic_profilePositionControl(Motor2, 0, vel_4, acc, dec, isRelative, islmmediately);
    bool result3 = harmonic_profilePositionControl(Motor3, 0, vel_4, acc, dec, isRelative, islmmediately);
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    if(result1 == 0 && result2 == 0 && result3 == 0)
        return 0;
    else
        return -1;
}

// Ctrl+C 信号处理函数
void signal_handler(int signum) {
    std::cout << std::endl << "Find the operator entered Ctrl+C, pause the current task, moving motors back......" << std::endl;
    MotorsMoveBack();
    harmonic_freeDLL(canid);  // 清理 Harmonic 动作
    std::cout << "Move back completed!!" << std::endl;
    exit(0);
}

// 单个电机延时函数
void MotorDelay(int MotorNum, int targetangle){
    while(1){
        int ActualPos;
        harmonic_getActualPos(canid, MotorNum, &ActualPos);
        if(targetangle * turn - 200 < ActualPos && ActualPos < targetangle * turn + 200)
            break;
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
    }
}

// 1电机的运行函数 -- 已废
// void Motor1Moving(int angle, int maximun){
//     if(-maximun <= angle && angle <= maximun){
//         for(int i = 0; i < 20; i++){
//             SingleMotorMovingControl(1, angle, vel_4);
//             MotorDelay(1, angle);
//             SingleMotorMovingControl(1, 0, vel_4);
//             MotorDelay(1, 0);
//         }

//         hint32 ActualPos;
//         harmonic_getActualPos(1, &ActualPos);
//         cout << "The actual position of motor 1 is " << ActualPos / turn << "°" << std::endl;

//         std::cout << std::endl;
//         mode_show(1);       // 运行完成后打印状态信息
//         std::cout << std::endl;
//     }
//     else{
//         std::cout << "[Error]Motor1Moving: The angle of motor 1 is out of range!!!" << std::endl;
//     }
// }

// 电机运动函数
void MotorMoving(int motornum, int angle, int maximun){
    if(-maximun <= angle && angle <= maximun){
        SingleMotorMovingControl(motornum, angle, vel_4);
        MotorDelay(motornum, angle);

        // 打印目前电机所在角度
        hint32 ActualPos;
        harmonic_getActualPos(canid, motornum, &ActualPos);
        cout << "The actual position of motor " << motornum << " is " << ActualPos / turn << "°" << std::endl;
    }
    else{
        std::cout << "[Error]MotorMoving: The angle of motor " << motornum << " is out of range!!" << std::endl;
    }
}
